import serial
import time
from threading import Thread

PERIPHERALS = ['actuators', 'ultrasound', 'encoders', 'ir',]

### peripheral : (input, output,)
CONNECTIONS = {
	'actuators' : (None, 'COM1',),
	'ultrasound' : ('COM1', None,),
	}


ULTRASOUND_FORMAT = 'HH'
ENCODER_FORMAT = 'hh'
IR_FORMAT = 'H'
CHECKSUM_FORMAT = 'H'
PACKET_FORMAT = ULTRASOUND_FORMAT + ENCODER_FORMAT + IR_FORMAT + CHECKSUM_FORMAT

## (@), <Ultrazvuk:4> XH, XL, YH, YL, <Encoderi:4> LH, LL, DH, DL, <IR:2> HIGH, LOW, <CHEKSUM:2> H, L  
	

class CommPipe:
	""" objec used by external logic
	"""
	def __init__(self, io, output):
		self._io = io
		self._output = output
	
	def write(self, data):
		self._io.write_to_port(self._output, data)
	
	def isr(self, data, time_stamp):
		""" owerride this in any way conviniant,
			will be in IO threadspace
		"""
		print 'ISR METHOD NOT OVERRIDEN !, DATA LOST: %s' % (data, )

class UltrasoundInput:
	def __init__(self, x, y):
		self.x = x
		self.y = y
class EncoderInput:
	def __init__(self, left, right):
		self.left = left
		self.right = right
class IRInput:
	def __init__(self, raw):
		self.receivers = []
		self.receivers.append( (raw & 0xC000) >> 14 )
		self.receivers.append( (raw & 0x3000) >> 12 )
		self.receivers.append( (raw & 0x0C00) >> 10 )
		self.receivers.append( (raw & 0x0300) >> 8 )
		self.receivers.append( (raw & 0x00C0) >> 6 )
		self.receivers.append( (raw & 0x0030) >> 4 )
		self.receivers.append( (raw & 0x000C) >> 2 )
		self.receivers.append( (raw & 0x0003) )


class IO:
	"""
	"""
	
	PACKET_LENGTH = len(PACKET_FORMAT) + 3 # start=1 + checksum=2
	STARTER = '@'
	
	def __init__(self, portname):
		self._portname = portname
		self._port = serial.Serial(portname)
		self._handlers = {}
		self._handlers['ultrasound'] = []
		self._handlers['encoders'] = []
		self._handlers['ir'] = []
	
	def _isr(self, port):
		chunk = ''
		while 1:
			new_chunk = port.read()
			if new_chunk:
				chunk += new_chunk
				time_stamp = time.time()
				while len(chunk) >= self.PACKET_LENGTH:
					if chunk[0] == self.STARTER:
						pack_data = chunk[1:self.PACKET_LENGTH]
						X, Y, left_encoder, right_encoder, ir_raw, checksum, = struct.unpack(PACKET_FORMAT, pack_data)
						US_data = UltrasoundInput(X, Y)
						encoder_data = EncoderInput(left_encoder, right_encoder)
						ir_data = IRInput(ir_raw)
						our_checksum = 0
						for x in pack_data: our_checksum += x
						if checksum != our_checksum:
							print '[checksum error] failed on %s <> %s ' % (checksum, our_checksum, )
						for x in range(self.PACKET_LENGTH): chunk.pop(0)
						## call handlers :
						handlers = self._handlers['ultrasound']
						for handler in handlers: handler(US_data, time_stamp)
						handlers = self._handlers['encoders']
						for handler in handlers: handler(encoder_data, time_stamp)
						handlers = self._handlers['ir']
						for handler in handlers: handler(ir_data, time_stamp)
					else:
						chunk.pop(0)
			else :
				time.sleep(0.0001)

	def attach_peripheral(self, peripheral_name, isr_method=None):
		""" 
		"""
		if self._handlers.has_key():
			handlers = self._handlers[peripheral_name]
			handler.append(isr_method)
			return True
		else:
			print '[IO] edit _handlers and add %s peripheral to it !' % (peripheral_name, )
			return False













